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Abstract Camera motion estimation from observed 
scene features is an important task in image processing 
to increase the accuracy of many methods, e.g. opti¬ 
cal flow and structure-from-motion. Due to the curved 
geometry of the state space SE 3 and the non-linear rela¬ 
tion to the observed optical flow, many recent filtering 
approaches use a first-order approximation and assume 
a Gaussian a posteriori distribution or restrict the state 
to Euclidean geometry. The physical model is usually 
also limited to uniform motions. 

We propose a second-order minimum energy filter 
with a generalized kinematic model that copes with the 
full geometry of SE 3 as well as with the nonlinear de¬ 
pendencies between the state space and observations. 
The derived filter enables reconstructing motions cor¬ 
rectly for synthetic and real scenes, e.g. from the KITTI 
benchmark. Our experiments confirm that the derived 
minimum energy filter with higher-order state differen¬ 
tial equation copes with higher-order kinematics and 
is also able to minimize model noise. We also show 
that the proposed filter is superior to state-of-the-art 
extended Kalman filters on Lie groups in the case of 
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linear observations and that our method reaches the 
accuracy of modern visual odometry methods. 
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1 Introduction 

1.1 Overview and Motivation 

Camera motion estimation is a fundamental task in 
many important applications (e.g. autonomous driving, 
robotics) in computer vision. It is an essential compo¬ 
nent of structure-from-motion, simultaneous localiza¬ 
tion and mapping (SLAM) and odometry tasks. Fur¬ 
thermore it aids as additional prior for e.g. optical flow 
methods. In the proposed approach, the ego-motion of 
the camera is fully determined solely by the apparent 
motion of visual features (optical flow) as recorded by 
the camera without needing additional sensors such as 
GPS or acceleration sensors. 

Although the camera motions can be reconstructed 
correctly from only two consecutive frames [5511^. the 
best performing methods take into account multiple 
frames. They are more robust against the influence of 
erroneous correspondence estimates. Two approaches 
to making use of the temporal context can be distin¬ 
guished: batch approaches - such as bundle adjust¬ 
ment methods - first record all the frames and 
fit in a smooth camera path afterwards. They some¬ 
times also incorporate loop closure constraints [48] to 
further improve camera motion accuracy. Factorization 
methods create the problem of jointly estimat¬ 

ing camera poses and scene points as a matrix decom¬ 
position problem. These batch approaches have the po¬ 
tential of working exactly as they make use of all avail- 
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able information. On the other hand, they hardly work 
in real-time applications, as the volume of incorporated 
information increases linearly with time. 

In contrast, online approaches apply sliding win¬ 
dow techniques lieim that track features on multi¬ 
ple frames to increase robustness and compute the best 
fitting motion. 

A mathematical description of (online) temporal 
smoothing is given by the notion of (stochastic) filter¬ 
ing m- in case of on the one hand, an ODE describ¬ 
ing the behavior of a latent variable, and on the other 
hand, observations that depend on the latent variable, 
the goal is to estimate the most likely value of the 
unknowns. However, stochastic filters suffer from non¬ 
linear dependencies of latent variables and observations 
as well as geometric constraints and unknown probabil¬ 
ity distributions. 

That is the reason why we chose deterministic Min¬ 
imum Energy Filters that do not need information 
about distributions and cope with the non-linearities 
of the observer equation and the geometry of the state 
space SE 3 in [5]. Since the state equation of the ego- 
motion in [ 9 ] is simple and requires small weights on 
the penalty term for the model noise, however, this ap¬ 
proach is sensitive against noise and requires good ob¬ 
servation data. 

Therefore, in this paper, we extend our previous 
work to a second-order model with constant acceler¬ 
ation assumption which is more stable and shows bet¬ 
ter convergence. In our experiments, we demonstrate 
significantly improved performance both on synthetic 
data with higher-order kinematic scenarios and on the 
challenging KITTI benchmark [18]. Comparison with 
novel continuous/discrete extended Kalman filters on 
Lie Groups [T^] shows that our approach - although 
being less general than m - leads to better results 
and is robust against imperfect initializations. 


1.2 Related Work 

Incorporation of temporal context - in terms of (par¬ 
tial) differential equations - into the estimation of la¬ 
tent variables has a long tradition in many common 
applications, e.g. robotics, aviation and astronautics. 
Starting from the seminal work of Kalman m con¬ 
sidering Gaussian noise and linear filtering equations, 
stochastic filters had have great success in many impor¬ 
tant areas of mathematics, computer sciences and engi¬ 
neering during the last fifty years. The filtering meth¬ 
ods have been improved during the last decades to cope 
with nonlinearities of state and observation equations, 
such as extended Kalman filters [23] , unscented Kalman 


filters [25] and particle filters [4] . For a detailed overview 
of these methods we refer to wm- 

However, one strong limitation of stochastic filters 
represents the fact that the a posteriori distribution 
is usually unknown and, in general, is infinite dimen¬ 
sional due to the nonlinear dependencies. To cover a 
large bandwidth of a posteriori distributions Brigo et 
al. approximated them by distributions of the exponen¬ 
tial family [13]. In contrast, particle filters try to sample 
from them [4]. Extended and unscented Kalman filters, 
on the other hand, only allow distributions that are 
Gaussian. 

Although these methods work successfully for many 
real-valued problems, they cannot be easily transferred 
to filtering problems which are constrained to man¬ 
ifolds, appearing in many modern engineering and 
robotic applications. Therefore, in the last decade, sev¬ 
eral strategies have been developed to adapt classi¬ 
cal unconstrained filters to filtering problems on spe¬ 
cific Lie groups and Riemannian manifolds: Kalman 
filters were transferred to the manifold of symmetric 
positive definite matrices [35]. Extended Kalman filters 
on SO 3 |32j with symmetry preserving observers m 
were elaborated. Particle filters on SO 3 and SE 3 were 
proposed in [29] as well on Stiefel [ 33 ] and on Grass- 
man manifolds [38] . An application of particle filters to 
monocular SLAM is reported in [3D]. 

Recently, unscented Kalman filters were generalized 
to Riemannian manifolds [ 22 ] . Since then, extended 
Kalman filters for constrained model and observation 
equations were developed [12] for general Lie groups 
based on the idea of the Bayesian fusion [3^. 

However, although stochastic filters have been 
adapted to curved spaces and non-linear measurement 
equations, they still require assumptions about the a 
posteriori distributions, e.g. to be Gaussian. Further¬ 
more, while transferring related concepts of probabil¬ 
ity theory and stochastic analysis to Riemannian man¬ 
ifolds is mathematically feasible [2311151114] . exploiting 
them computationally for stochastic filtering seems in¬ 
volved. The widely applied particle filters also have lim¬ 
itations in connection with manifolds since the sampling 
requirements of particles become expensive [30] . 

A different way to approach a solution to the filter¬ 
ing problem was proposed by Mortensen [33]. Rather 
than trying to cope with the probabilistic setting of the 
filtering problem, he investigated the filtering problem 
from the viewpoint of optimal control. By using the 
control parameter to model noise and by integrating 
a quadratic penalty function over the time, he found 
a first-order optimal Minimum Energy Filter. The ad¬ 
vantage of this method is that it does not rely on as¬ 
sumptions about, or approximations of, the a posteriori 




Second-Order Recursive Filtering on the Rigid-Motion Lie Group SE3 Based on Nonlinear Observations 


3 


distribution and that Hamilton-,Jacobi-Bellman equa¬ 
tion provides a well-defined optimality criterion. It was 
shown theoretically in |28j that the minimum energy 
estimator converges with exponential speed for control 
systems on K" that are uniformly observable. 

The first article applying the minimum energy fil¬ 
ters to geometrically constrained problems used per¬ 
spective projections in the case of vectorial measure¬ 
ments |3] . The minimum energy filters were generalized 
to second-order filters on specific Lie groups with the 
help of geometric control theory in [26112113^ . The Min¬ 
imum Energy Filter, as introduced by Mortensen |33j . 
was generalized to the Lie group SO 3 for the case of 
linear observation equations and for attitude esti¬ 
mation m- Further follow-up work [30] generalized the 
filter to non-compact Lie groups sa¬ 
in this article, we greatly elaborate our initial work 
on camera estimation using nonlinear measurement 
equations, especially by moving from a constant veloc¬ 
ity assumption |9] to a second-order state equation with 
constant acceleration model. In addition, we investigate 
generalized kinematic models of arbitrary order. 


1.3 Contribution and Organization 

Our contributions reported in this paper amount 

— to generalize the constant camera velocity model 
from [^ (non-linear measurement model) to polyno¬ 
mial models, in particular the constant acceleration 
model; 

— to provide a complete derivation of the second-order 
minimum energy filter [41] as applied to camera mo¬ 
tion estimation together with robust numerics that 
are consistent with the geometry and the structure 
of matrix Riccati equations; 

— to report experiments demonstrating that higher- 
order kinematic models are more accurate than the 
constant velocity model [3] on synthetic (with kine¬ 
matic camera tracks) and real world data and that 
they enable to reconstruct higher-order information; 

— to report experiments comparing our approach to 
state-of-the-art extended Kalman Filters on Lie 
groups [T^, indicating that our method is supe¬ 
rior in coping with non-linearities of the observation 
function as well as in being more robust against im¬ 
perfect initializations. 

In the next section, we introduce the filtering equa¬ 
tions related to our problem of camera motion recon¬ 
struction. Next, we describe the basics of minimum 
energy filters and detail how to apply the (operator¬ 
valued) minimum energy filter derived from |41| to 
our scenario. The numerical integration schemes of the 


ODEs for the optimal state will be given in Section]^ 
We will confirm the theoretical results in Section Hjby 
experiments on synthetic and real world data and thus 
underline the applicability of our approach. 


1.4 Notation 


GL4 

SO3 

SE3 

SZ 3 

veCsc : 6^3 ^ 
mats c = vecjc^ 

g 

8 

Tag 

vecg : 0 —> 
matg =: vec^ ^ 

Expg 
Logg 
Pr : R4 x4 
LqH := GH 
ThLg 

Gq := Ti^LgV 
G-^q ■.= TuL*^q 
Id 

(G??>g 

(x,y) 

V.- 

UJ^ := ui{x,q) ■■= 

Ul — q ■- UJriX 

■= {v,‘-^xO 
{uj-*q,0 ■■= {q,‘^iX) 


d/(G) 
df{G)[q] 
Hess /(G) 


d,/ 


dc 

W := {1, 

Xi:j 

tn 

||x||^ := {x, Qx) 


General Linear group 
Special Orthogonal group 
Special Euclidean group 
Lie algebra of SE 3 
vectorization operator 
inverse of vecs c 
(product) Lie group SE 3 xR® 

Lie algebra of g 
tangent space of 5 at G 
vectorization operator 
inverse of vecg 
exponential map on Q 
logarithmic map on Q 
projection onto Lie algebra ses 
left translation 

tangent map of left translation at H 
shorthand for tangent map 
shorthand for dual of tangent map 
identity element of Lie group 
Riemannian metric at G S 5 
Riemannian metric on Lie algebra g 
scalar product on R" 

Levi-Givita connection on Tg 

connection function for x, r; S g 

swap operator 

dual of connection function 

dual of swap operator 

Lie bracket on considered 

Lie group, matrix commutator 

differential/Riemannian gradient 

of / at G 

directional derivative of / 
in direction q 

Hessian of a twice differentiable 

function f : g ^ R 

differential resp. 

i-th component of / 

differential of an expression resp. G 

set of integer numbers from 1 to n 

tangent vectors 

i-th to j-th component of x 

block matrix with rows from i to j 

and columns from k to I from A 

n X n identity matrix 

quadratic form regarding Q 

i-th unit vector in R" 


Moreover, we will employ the following concepts 
from differential geometry: 


Riemannian metric on product Lie group. On SE 3 as 
submanifold of GL 4 , the Riemannian metric at F S SE 3 
for ^,r]£TE SE 3 is given by (^, t?)^ := (F"!^, 
where := iv{A'‘ B) is the usual inner matrix 

product. 
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Fig. 1: Illustration of the Lie group Q (represented as sphere) with its Lie algebra g and tangent spaces at different 
points. A tangent vector x at a point G can be expressed as tangent map at identity of the left translation at G 
of a vector ry S 0 , i.e. y = Gry. Since the Lie algebra g can be identified by the veCg mapping with we can 
express each tangent vector at a point G as a pair (G, veCg(7y)). Each tangent vector on any tangent space may be 
mapped to the manifold using the exponential map Exp. 

Riemannian Gradient. For a real-valued function / ; 

^—>■151, the Riemannian gradient d/(G) is defined 
through the relation (d/(G),ry)G := d/(G)[?y] for all 
T] G TcG- For the product Lie group G = SE 3 xK® and 
G = {E,v) G G,ri = {Er]i,T] 2 ) G TqG we calculate the 
Riemannian gradient as follows: 

d/(G)[ry] = (d/(G), ry)G 

= {E-^dEf{iE,v)),r]i)t^ + {dyf{{E,v)),r]2) , 

where dEf{{E,v)) is the partial Riemannian gradient 
on SE 3 and dyf{{E,v)) is the Euclidean partial gradi¬ 
ent on K®. 


Levi-Civita connection and connection function. For 
G G G we denote by V the Levi-Civita connection of 
the Lie group G given through V : TqG x TqG -G TqG, 
with the properties symmetry, i.e. [rj, x] = '^tjX ~ 
where [•, •] denotes the Lie bracket, and compatibility 
with the Riemannian metric. The Levi-Civita connec¬ 
tion is characterized by its connection function w : 
0 X 0 —>■ 0 , := uj^r] := VjTy with the property 

Vc^G'q = Gwjry for ?y G g. 


Riemannian Hessian. The Riemannian Hessian is de¬ 
fined through (Hess/(G)[^],ry) := d(d/(G)[/])[ry] - 
d/(G)[V,,^]. On the product Lie group G = SE 3 xM® 
which we consider in this paper, we set G = {E, v) G G 
and ^ = (E/1,^2) G TgG, V = {Em,m) G TqG. 


2 Minimum Energy Filtering Approach 


2.1 State Model with Constant Acceleration 
Assumption 

In the following we will denote by E{t) G SE 3 the time- 
dependent (external) camera parameter that can be ex¬ 
pressed in terms of a rotation matrix R{t) G SO 3 and 
a translation vector w(t) G as a 4 x 4 matrix 


Eit) = («« "’<*> 

V^lx3 -L 


( 1 ) 


for which we also use the shorthand E(t) = {R{t), w{t)). 
Since the ego-motion of a camera is generally not con¬ 
stant, the model E = 0 assumed in previous work [9] 
does not hold in real-world problems, where a camera 
fixed to a car rotates and accelerates in different direc¬ 
tions. The constant acceleration assumption, however, 
is more suited in this cases. It can be described by the 
second-order differential equation E{t) =0 for all t with 
initial pose E{to) = Eq and velocity E{to) = Vq. In 
general, one can consider a polynomial model of even 
higher-order for E{t). In the following, we will focus 
on the assumption that Eft) is quadratic in t. We will 
comment on generalizations at the end of Section]^ 
The equation E{t) = 0 can be prescribed as a sys¬ 
tem of first-order differential equations 


m =v{t) , 
v{t) = 0 , 


( 2 ) 


where V{t) G Teyt) SE 3 and V{t) G ryp)T£;(t) SE 3 = 
Teu) SE 3 . However, since the tangent bundle of a 
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Lie Group can be expressed in terms of the product 
TSE 3 ^ SE 3 xse 3 , we obtain a more compact expres¬ 
sion, i.e. 


E{t) =E(t) matse(u(t)), 
v{t) =06 G M®, 


where the operator mats 


: —>• se 3 is defined by 

/ n mm \ 

/ ° -75 ’'M 






V3 

V2 

m 

V2 

0 


' 

0 

m 

V2 

0 


0 r?6 
0 0 > 


(4) 


The inverse operation is denoted by veCse : se 3 —)■ K®. 
Note that this operation is consistent with the usual 
scalar product, i.e. for x, ?7 G se 3 it holds 


(X,»?)id :=tr(x^r7) = (veCse(x), veCse(??)). (5) 


Since SE 3 is a Lie Groups regarding the matrix multi¬ 
plication and K® is a Lie Group regarding addition, we 
can understand the system ([^ as a first-order differen¬ 
tial equation on a product Lie Group 

g := SE 3 ( 6 ) 

For two elements Gi = (Ei,ui),G 2 = (£’ 2 ,^ 2 ) G ^ we 
define the left translation by Lg^G 2 '■= {EiE 2 ,vi + 
V 2 ) G g. Since the tangent bundle TK® can be identified 
with M®, we obtain the Lie algebra 

fl=se3xIR®. (7) 

In turn, we can take down @ compactly as 

G{t) = {E{t) mats,(v(t)), Og), ( 8 ) 


where E and v will denote the first and second ele¬ 
ment of G G 0, respectively. On matrix Lie groups, one 
can express kinematics directly as matrix multiplica¬ 
tion (cf. [U]), i.e. E = EE for E G se^jE G SE 3 , which 
is not valid for general Lie groups. The rigorous way to 
describe kinematics is to use the tangent map (cf. |41) 1 
of the left translation which is given by the following 
proposition: 

Proposition 1 The tangent map of the left translation 
regarding G = {E,v) G g at identity, i.e. Ti^Lq '■ ^ 

Tog, can be computed for 77 = (771,772) G g as 


Remark 1 During the further development, the nota¬ 
tion Gr] for a Lie group element G G g and r? G g 
must always be understood as the tangent map of 
the left translation at identity. Similarly, we denote 
G~^r] := TmLqT] for the dual of the tangent map of 
Lg at identity. 


2.2 Optical Flow Induced by Ego-Motion 


The optical flow u : f2 x T —)■ on an image se¬ 

quence {I(t),t G T} can be computed in terms of the 
underlying scene structure as given by a depth map 
d : n X T and the camera motion E : T SE 3 , i.e. 
Elt) = {R{t),w{t)), where R{t) and w{t) denote the 
camera rotation and translation, respectively, by the 
following relation: 

u{x,t;d{x,t),{R{t),w{t))) 

=Tr{R{ty {{f) d{x,t) - w{t))) - X, 


whereas tt : 


is the projection {xi,X 2 ,X 3 )^ 1 —)■ 


xf^{xi,X 2 y‘ as depicted in Fig.^ Note that x G in¬ 
dicates inhomogenous coordinates rather than homoge¬ 
nous coordinates on the projective space. 

We can also express (12) directly in terms of E{t). 
By adding the superscript k for (discretized) pixels x^ G 
f2, we obtain 


u{x^,t-d{x,t),E{t))-\-x'" =Tr{{E ^(t)5fc(i))i:3), (13) 

where gk{t) := (fi(x^, t)(x*)^,d(x^, t), 1 )^ denotes the 
data vector containing depth information of pixel x^ 
below. 


Remark 2 In the equation (13) we assumed a static 
scene, since we set the scene point X constant in time. 


3 Minimum Energy Filter Derivation 


In this section, we will determine the problem of camera 
motion estimation with filtering equations, and we will 
summarize the most important steps for the derivation 
of the minimum energy filter. 

By denoting the left hand side of (13) by pk G 
which is the observation, i.e. 


TuLgV = {Epi, 772 ) = L(^e,o) 11 =■ Grj. (9) 

With Proposition we can write down ([^ as 
G{t) = rid£G(t)/(G(t)) = G{t)f{G{t)), (10) 

where / : ^ g is given by 

/(G) = /((£, v)) = (matse(u), Og). (11) 


yk{t) := u{x^,t-, d{x, t), E{t)) -b x'", 
and defining 

hk{E,t) := 7r((£"^(t)5fe(t))i:3) 


(14) 


(15) 


as the right hand side of (13), together with (§ 
and (10), we obtain the following state- and observa¬ 
tion system by setting G = (£, u) G 
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Fig. 2: Camera model for the monocular approach: A 
static scene point X is projected onto the plane at x{t) 
of the first camera E{t — 1) which is mounted at the 
origin with rotation I 3 such that X = d{x{t))x{t). By 
moving the camera into position E{t) = {R{t),w{t)), 
the scene point is projected onto Tr{R^{t){X — w{t))) = 
x{t+l) which is at the same (relative) image position as 
i;(t +1) on the second image plane. The induced optical 
flow is given by the difference u{x{t), t) = x{t+l) — x{t). 


G{t) =G{t){f{G{t)) + <5(t)), G(to) = Go , (state) (16) 
Vk =hk{E{t),t) +€k{t),k G [n], (observation) (17) 

where /(G) is defined as in 0 and n denotes a 
(fixed) number of specific image pixels. The functions 
6 : T ^ g and ek ■ T ^ , k G [n] are noise processes 

that model deviations from state and observations, re¬ 
spectively. Here, T denotes a continuous time interval, 
e.g. r = M>o. 


3.1 Energy Function 


Given a depth map, which is contained in the function 
gk{t) in (151 and the optical flow Uk in terms of the 
observations yk in (14), we want to find the camera 
motion and its velocity in terms of G{t) G Q such that 
the observation error ek in 0 is minimal and such 
that (16) is fulfilled with minimal deviations 6{t) for all 
t G T. 


To this end, we consider the penalization of (5 = 
(( 5 i,i 52 ) G g and e = {ek}k-i by a quadratic function 


c : g X x T x T — >• K given as 

c{6,€,T,t) :=i(||vec5e(<5i(T))|||^ 

n 

+ I1^2(t)||| 2 -I- 0||efc(T)||Q) , 

fc=l 


(18) 


where Si, S 2 G 


3x6 


and Q G 


p2x2 


are symmetric, 


positive definite weighting matrices. From |41] we adopt 
the idea of a decay rate a > 0 , and thus we introduce 
the weighting factor on the right-hand side 

of dTsl: 


c(S,£,T,t) :=^e ‘“^f||vecse(^i(r 


I Si 


||52(r)|||2-bX]lkfc(r)ll^). 


fc = l 


(19) 


Based on the penalty function (19), we define the en¬ 
ergy: 

J(S,e,to,t) := mo(G(t),t,to) + f c{6,e,T,t) dr , (20) 

Jto 


where toq is a quadratic penalty function for the initial 
state. For our model we set 


mo(G, t, to) := (G - Id, G - Id)M, (21) 

where the difference is canonical, i.e. G— Id = (A—II 4 , v) 
for G = {E,v). 


Remark 3 Instead of using two quadratic forms with 
matrices Si, S 2 , we can use more generally a symmetric 
and positive weighting matrix S G if we want 

to couple and 32 - In the upper case we find that 


3.2 Optimal Control Problem 


The optimal control theory allows us to determine the 
optimal control input <5 : T —^ g that minimizes the 
energy J{S,e{G{t),t),to,t) for each t G T subject to 


the state constraints (16). To be precise, we want to 


find for alH G T and fixed G{t) the control input 
defining 


V{G{t),t) := min J{6, e{G{t),t),to,t), s.t. ([T6| . (22) 


The optimal trajectory is 


G*{t) := argminc.(t)gg V(G(t),t), (23) 

for alH G T and V(G, to) = mo (Go, to, to). This prob¬ 
lem is a classical optimal control problem, for which the 
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standard Hamilton-Jacobi theory |26L [5] under appro¬ 
priate conditions results in the well-known Hamilton- 
Jacobi-Bellman equation. Pontryagin proved that 
the minimization of the Hamiltonian provides a solution 
to the corresponding optimal control problem {Pontrya- 
gin’s Minimum Principle). 

However, since C/ is a non-compact Riemannian 
manifold, we cannot apply the classical Hamilton- 
Jacobi theory for real-valued problems (cf. [ 5 ]). Instead 
we follow the approach of Saccon et al. [41] who derived 
a left-trivialized optimal Hamiltonian based on control 
theory on Lie groups This left-trivialized optimal 
Hamiltonian is defined hy 'fL~ i^xgxgxT—i-M, 


n (G, /r, 5, t) := c{5, e(G, t),to, t) - {p., F{G{t)) -b (5)id- 

(24) 


The minimization of p4| ) w.r.t. the variable 6 = (di, < 52 ) 
leads im Proposition 4.2] to the optimal Hamiltonian 


n-{G,p,t) ■.= n-{G,p,5*,t), 

where S* = is given by 

vec5c(i5*) = veCsc(7ti), and 




(25) 


(26) 


2 • 


Examining the right-hand side of (25) in detail, we ob¬ 
tain 

n 

n-{{E, V),p, t) = - hk{E)\\l) 

k^l 

- (^(/ri, matse(S'r^ veCse(/ri)))id (27) 

+ { P 2 , V2)) - (711, matse(R))id , 

where we used e{G{t),t) = {Vk — ^)}fe=i- Here 

we introduced on the left hand side the variable G since 
the right hand side depends on G = {E,v). 

In the next section, we will compute explicit ordi¬ 
nary differential equations regarding the optimal state 
E*{t) for each t G T that consists of different deriva¬ 
tives of the left trivialized Hamilton function (27). 


3.3 Recursive Filtering Principle by Mortensen 

In order to find a recursive filter, we compute the total 
time derivative of the optimality condition on the value 
function, which is 

diV(G*,t) = 0, (28) 

for each t G T. This equation must be fulfilled by an 
optimal solution G* G ^ of the filtering problem. Un¬ 
fortunately, because the filtering problem is in general 


infinite dimensional, this leads to an expression contain¬ 
ing derivatives of every order. In practice (cf. [5Tll4Ij L 
derivatives of third order and higher are neglected, since 
they require tensor calculus. Omitting these leads to a 
second-order approximation of the optimal filter. The 
following theorem is an adaption of im Theorem 4.1]: 


Theorem 1 The differential equations of the second- 
order Minimum Energy Filter for state (16) and non¬ 
linear observer model (HTj) are given by 


(G*)- 1 g* = (/(G*)-mat,(P(t)veCg(rt(G*)))), 
G*(to) = Id, (29) 

P{t) = -a-P + S-^ +GP + PG^ 

Z)fc=l(A^eCsc(Pr(Afc(£*))) + Dk{E*)) Ogxe^ 


- P 
P{to) = II12 


J6x6 


^ 6 x 6 


P, 


(30) 


where rt{G*) := (X)fc=i ^e) and 

-F{G*,t) le 


G(G*,t) := 

with 


J6X6 


J 6 x 6 


F{G*,t) ■■=ad:TMG*))+r:ec,pPrPG^))- (31) 

The function Ak : SE 3 —)• is given by 


Ak{E) 


Ak{E,gk) := I - IE-^tdllY 

■ QiVk - hkiE))gJE-^, 

(32) 


3x6 


where Kk ■= Kk{E) := (e|)^P“^gfe. The second-order 
operator Dk : SE 3 -G is given by (101), see Ap¬ 

pendix 

The matrix valued functions FziFf : M® — 
are obtained from the vectorization of the connection 
functions. Their components are given by {Fz)ij '■= 
Z)fe=i Pjk^’' iPz)ik ■= Z)j=i Pjk^^ z G K® and 
the Christoff el-Symbols F^f. are given in Appendix 


This theorem will be proven at the end of the section. 


Remark 4 A generalization of this theorem is published 
in Saccon et al. |4T| for a larger class of filtering prob¬ 
lems. However, the application of the theorem is not 
straightforward since the appearing expressions, e.g. ex¬ 
ponential functor, cannot be evaluated directly. Fur¬ 
thermore, the adaption to nonlinear filtering problems 
has not been considered in the literature yet. Besides, 
we show how to find explicit expressions in terms of 
matrices for the general operators in m- 
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In our previous work [9] we presented a theory re¬ 
garding the case of constant velocity. This theory can 
be derived directly from Theorem by neglecting the 
velocity v i.e. the second component of G = {E, v) € Q 
(thus changing from Lie group SE 3 xK® to SE 3 ) and by 
setting /(G) = 0. In this case, the state and observation 
equations are reduced to 

E(t) =E{t)S{t), E{to) = Eq, (state), (33) 

Vk =hk{E{t),t) + €k{t), k £ [n]. (observation). 

(34) 

For the reader’s convenience, we state the theory under 
the assumption of constant velocity as a corollary: 

Corollary 1 The differential equations of the second- 


order Minimum Energy Filter for our state (33) and 
nonlinear observer model 


are given by 

n 

{E*)-^E* = -mat„(P(i)vec,e(5]Pr(Afe(E;*)))), 




E*{to)=ld, 

P{t) = -a-P + 

-p* p{r* 

veCsc((-S ) '-E*) ^ Vi 






-P{ 


fc=l 


(Pvec,.(Pr(Afc(£;*))) + Dk{E*)))P, 


(35) 


(36) 


P{to) — le- 

Remark 5 We compare the computational complexity 
for the cases of constant velocity and constant accelera¬ 
tion. By considering the difference between Theorem [l] 
and Corollary [2 we see that the only differences are a 
larger state space and the occurrence of the additional 


operator /(G*) in (31). However, this does not change 
the computational effort significantly. Thus, we suggest 
to use the second-order minimum energy filter since it 
is more robust but computational only slightly more 
complex as we will see in the experiments. 

Before we will turn to proving Theorem we first 
provide some lemmas that are based on the general ap¬ 
proach of [41]. However, we cannot use the main result 
of m] directly, since the appearing general operators 
are complicated to evaluate. Instead, we provide the 
corresponding expressions in such a way that they can 
be easily implemented. Thus, following m Eq. (37)] 
the estimate of the optimal state G* is given by 

(G*)-ig* ^ _d2-H-(G*,0,t) 

- o (G*)-Mi-H-(G*,0 ,t). 

This expression contains the second-order information 
matrix Z{G,t) : g —g of the value function V as de- 


(37) 


fined in ( 22 ), defined through 


An explicit expression for the gradient of the Hamilto¬ 


nian in (37) is provided in the following lemma: 


Lemma 1 The Riemannian gradient diH (G, /i, t) on 
TgQ for G = (E, v) can be calculated as 


diH {G,fj,,t) 


n 

=G(e~°‘^*~*°^ ^ Pi{Ak{E)), - vec5c(Mi)), 


(39) 


k=l 


where the function Ak{E) = Ak{E,gk) : SE 3 xl 


GL 4 is defined in (32). 


By insertion of (39) in and usage of the definition 
of rt{G*) from Theorem!^ we obtain 


(G*)-ig* = -d 2 'H-(G*, 0 ,t) 




^°'>Z{G\t)-^ ort{G*). 


(40) 


Following the calculus in m, the evolution equation 
for the trivialized Hessian Z{G,t) : g —>■ g* is given by 


dt 


Z{G*it),t) 
iZ{G ,t) O 
-\-Z{G ff) ° 

+ o Z{G* ff) 

+ ridL^* oHessiH-(G*,0,t)oTidTG* 

+ TidL^. o d 2 (diH-)(G*, 0, t) o Z(G*, t) 
+ Z{G\t)o di(d 2 H-)(G*, 0, t) o TmLg- 
-k Z{G*,f) o Hess 2 n~ (G*, 0, t) o Z{G* ff). 


(41) 


(cf. mi Eq. (51)]). 

The “swap”-operators 
sion are defined in Section 


1.4 


in this expres- 
i.e. lofff := Lo^rj and 
:= {f.,oj^x)id =7^Wx77)id- By considering 
the standard basis of g, there exists a matrix represen¬ 
tation K G such that for all rj = (ryi, 772 ) € g we 

receive 


veCg{Z{G*,t) or]) = K{t)veCg{r]). 


(42) 


Similarly to |9] we need to evaluate the right-hand side 
of the evolution equation at 77 G g and to vectorize 
it. The single expressions are shown in the following 
lemma. 

Lemma 2 (Matrix representations of Z) 


Let Z{G*,t) : Q ^ g be the operator (38). Then there 
exists a matrix K = K(t) £ yielding 


(38) veCg{Z{G*,t){r])) = K{t)vecffri), 


Z{Gff)or] = G ^ oHessi H(G,t)[G 77 ]. 


(43) 
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and thus 

veCg{d/dtZ{G*,t){r])) =k{t)veCg{r]), ( 44 ) 

veCg{Z-^{G*,t){T])) =K-^{t)veCg{r]), ( 45 ) 

as well as 

1 . veCg(Z(G*,i) oa;(e.)_ig.77 

-b Z(G*,i) g^j^ry) = K{t)Bvec^{r]) 

2. veCg(a;*Q.^_i^. oZ(G*,it)o77 

+ ^Sw-(G*.o.t) ° Z{G*,t) 0 7])= B^K{t) vecg(77) 

3. veCg(TidLG* o Hessi'H~(G*,0,t)[TidiG*'f/]) = 

g-a(t-to) 

. /^I]fe=l(Aec„(Pr(Afc(B))) +Dk{E)) 06x 6\ yeCg (t?) 

V Ogxe 06x6/ 

4. veCg(Z(G*,i) odi(d2'H“)(G*,0,i) oTidLc-??) 

= -K{t) ^ vecg(77) 

\^6x6 ^6x6/ 

5 . veCg(TidL^. od2(diH-)(G*,0,t)oZ(G*,t)or;) 

06x6 06x6 1 ^ 

Wi:(i)vecg(77) 

Jl6 <J6x6/ 

6 . veCg(X(G*,i)(Hess 2 ^-(G*,0,i)[X(G*,t)(ry)])) 

= YeCg{ri), 

with r., r*, and functions Ak, Dk from Theorem^ and 


where on the right-hand side we assume that Kft) is an 
approximation of the vectorized operator Z(G*{t),t)- 
This is the reason why we replace the approximation 


by an equality sign in (49). With a change of variables 
(cf. [H]) 


P{t) := e-“(*-‘«)it:(t) 


-1 


(50) 


and the formula for the derivative of the inverse of a 
matrix we obtain 


P(t) = 


= - 


*°^K(t)-^ - K{f)-^k{f)K{t)- 


= - aPft) - e‘^^*-*°'>P{t)k{t)P{t). 


(51) 


Insertion of (49) (after omitting the direction veCg{r]) 


that was chosen arbitrarily) into (51) leads to the dif¬ 
ferential equation ( [M| in Theorem ^Therefore, we also 
find that 


GG-.<) = (;:::o.':.)-B(f). 


(52) 


The differential equation of the optimal state (29) 


follows from inserting (50) into (48), which completes 
the proof. □ 


r> _ f Oexe 

V ** 6 x 6 0 ex 6 


with E from Theorem^ 


). 


(4®) 3.4 Generalization to Higher-order Models 


With these lemmas we are able to prove our main 
result in Theorem [T] 

Proof (of Theorem We can easily compute the dif¬ 
ferential of Hamiltonian in (12^ which is 


-d2P-(G%0,t) = (mat,e(^**),0) =/(G*). (47) 

By inserting expression (|4^ into the optimal state 


equation (40) together with the definition of the oper¬ 


ator veCg(X(G*,t) ^ o G*r]) = K ^{t)veCg{r]), we find 
that 

{G*)-^G* = f{G*) 

- matg(veCg(X(G*,t)“^ o rt(G*))) 

=f{G*) - matg(X-i(t) veCg(rt(G*))) . (48) 

The application of the veCg —operation onto the equa¬ 
tion ( [4I]) evaluated for a direction 77, together with 
Lemma |2] results in 

k{t)vecg{r]) = Kft)B + B^ Kff) 


_|_ g-“(t-to) ^ Xfc = l (Geese (Pr{ 2 lfc(E)))+-Dfc(B)) 06 x 6 


Oex 


Oex 


- 75 V)(s:::o;;.)-(”sr£:)E(i) 

_ e“(‘-*°)i4r(t)S'-^iL(t) veCg{r]), 


(49) 


In the previous section, we discussed minimum energy 
filters to estimate ego-motion under the assumption of 
constant acceleration. We saw that changing the as¬ 
sumption of eonstant veloeity to constant acceleration 
requires extending the Lie group and adopting the func¬ 
tions /(G) and G(G). 

The generalization to higher polynomial models re¬ 
garding camera motion, where we assume that the m-th 
order derivative of the ego-motion should be zero, i.e. 


dt^ 


-Bit) = 0, 


(53) 


is straightforward. 

Again, the approach can be described by a system of 
first-order ODEs as follows. Note that in the constant 
acceleration model (second-order), only the first-order 
model needs to respect manifold structures, whereas 
all the other derivatives are trivial since they evolve on 
Euclidean spaces: 


Eft) =Eft) (matje G (t) -I- 5i (t )), 

Vlft) =V 2 ft) + 62 it), 

rm—2fl) —^m —1 “b 1 (^), 

’hm—lfl) ~^mfl) 


(54) 
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To achieve a unique solution we require initial values, 
i.e. i;i(0) = , Um-i(O) = v^_i G K®. Again, the 

observation equations ( [l7| stay unchanged. The min¬ 
imum energy filter for this model is provided by the 
following theorem. By using once again 


G = {E,Vi, . . G Gm ■= SE 3 xM® X • • • X 


(55) 


the corresponding minimum energy filter can be ob¬ 
tained easily from Theorem 

Theorem 2 (Minimum energy filter for m— th 
order state equation) The differential equations of 
the secon d-or der Minimum Energy Filter for t he state 
equation ( [54| and the observation equations 0 are 
given by the equations (29) and 


P{t) = -a-P + S-^ +CP + PC^ 

_ p /^Z)fc=l(-^vec.,(Pr(Xfc(B*))) + Dk{E*)) Ogx(m-l)6 


0 


(m —1)6x6 


0 


P{io) = ISm , 


(m—l)6x(m—1)6 

(56) 


where we assume that the expressions G* and P lie in 
the spaces Gm and respectively. The appearing 

expressions in Theorem [7] are replaced by 

f{G) :=(mat5e(i’i),W2, • ■ •, Oexi), 

n 

rt{G*) :=(^Pr(Afe(£;*),0(„_i)6xi), 
k=l 

y Og(„_ 2 )x 6 ) 


G{G*,t) := 


OsxS 06x6(m— 1)2 


All the other expressions from Theorem [7] stay un¬ 
changed. 

Proof Since product Lie groups are simply Lie groups 
with the product topology, we can still apply the gen¬ 
eral minimum energy filter of Saccon et al. [41] . The 
Lie group Gm has dimension 6m such that the vec¬ 


torized bilinear operator Z from (38), i.e. P results in 


a 6 m X 6 m matrix. The definition of the function / 


follows from the differential equations in (54). Simi¬ 


larly to Theorem the observations do not depend 
on the whole state G = {E,vi, ... ,Vm-i), but only 
on E. This leads to the fact that rt, which is essentially 
the left-trivialized differential of the Hamiltonian (i.e. 
G~^di'H~ {G,0,t)), vanishes after calculating the dif¬ 
ferentials regarding ui,..., Vm-i- Similarly, the Hessian 
G~^ Hessi ?i~ (G, 0, t) [Gt]] in Lemmaj^can be extended 
by zeros. Furthermore, components vi,... ,Vm-i G 
have a trivial geometry and do not contribute to cur¬ 
vature and thus the corresponding connection functions 


in Lemmaj^also do not influence curvature. Finally, we 
can compute the expression 

di (d 2 H-(G,0, t)) [Gry] = -d/(G) [ 77 ] 

= -(matse(^^2),^'3, ■ • ■,'^^- 1 , 0 ) 

and thus 

veo,(d,(d,«-(G,0,i))|G,|) = (“X;r' 

as we did in Lemma for the special case. Together 
with the adjoint operator in T'{G,t), we obtain the ex¬ 
pression G. □ 


4 Comparison with Extended Kalman Filters 

As an alternative to the proposed approach, we also 
suggest considering extended Kalman filters. For this 
purpose, we will compare our approach to a state-of- 
P the-art discrete / continuous extended Kalman filter on 
He groups |12j in Section|^ The Kalman filter approach 
is valid in a more generalized scenario compared to 
ours because the state space as well as the observation 
space are matrix Lie groups, whereas we only consider 
real-valued observations in K". On the other hand, one 
needs to know that the covariance matrices of the model 
and observation noise and the a posteriori distribution 
are assumed to be Gaussian, which is in general not 
true for non-linear observation dynamics. 


Algorithm 1 Extended Kalman Filter for Lie Groups 

Require: State G(q_i), Covariance P(t;_i), Observations 
yk{ti},k = 1 ,. .. ,n 

1: procedure Propagation on : Integrate the follow¬ 

ing differential equations 
2: G{t) = G{t)f{G(t)) 

3: P(t) = J(t)P{f)P P{f){J(t)y P S 

-|-|E(ad0 (e(t))Sadg (e(t))^) 

+ YE(ad0(c(t))2)5+Y5E(ad,(e(t))2)^ 

4: G-{ti)=G{ti), p-{ti)=P{ti) 

5: procedure Update: 

6: Ki = P-{ti)Hj {HiP-{ti)Hf -HQ;)"" 

7; = Ki Efc=i(27fc(b) - h\g-{ ti))) 

8: G{ti) = G-(i;)Exp(mat0(m“ )) 

9: P{t,) = <f(m-)(li2 - KiHi)p-{ti)<I>{m-^y 


The extended Kalman Filter from [12] is summa¬ 
rized in Algorithm and has already been adapted to 
our problem for real-valued observations. In line the 
residual is expressed as direct difference which is a spe¬ 
cial case of [12] . The function <I> in line on ^ is shown 
in Appendix jE] 

In the next section, we will adapt the Algorithm 
to different scenarios: to a filtering problem with linear 
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observations as well as to our nonlinear filtering prob¬ 
lem with a projective camera (cf. 0 ). 

Remark 6 Note that the extended Kalman filter 
from [12j requires a differential equation (that is not 
only driven by noise) in order to propagate the state, i.e. 
E{t) = E{t){f{E) + where / is non-trivial. Oth¬ 
erwise the update step of the extended Kalman filter 
is not significant because update and correction steps 
in the extended Kalman filter are separated. This is 
the reason why we only compare it to the second-order 
model where / ^ 0 . 


4.1 Derivations for Linear Observations 


In the scenario of linear observations the state equa¬ 
tion stays unchanged, i.e. is identical to (16). Similarly 
to m we use the following linear observation equations: 


Proposition 3 The Extended Kalman Filter for the 
constant acceleration model 0 and linear observation 
equations (57) is given by Algorithm^where the matrix 

Hi ■= YZ=1 


m 


/vec^^fPr{E {tiyeiaj)y OixeX 
vec^^(Pi{E{ti)^ejaj)y Oixe 
veCse(Pr(L;(ti)^e|a^))^ Oixe 
\vec^^(Fi{E{tiyelaj)y Oixe/ 


(61) 


and the function J{t) Eq. (52)]) is provided 

by (114) in Appendix 


Remark 1 Note that ( [hlj ) is different from [121 
Eq. (Ill)] because of the additive instead of multiplica¬ 
tive noise term, and consequently is not consistent with 
the group structure of SE 3 . 


4.2 Derivations for Nonlinear Observations 


yk{t) = E{t)ak + €k{t), fce[n], (57) 


where E{t) S SE 3 is the first component of G{t) G G 
and Ofc G are vectors that model the linear trans¬ 
formation of the state G. Again, ek{t) G are the 
observation noise vectors. 

In this case, the Minimum Energy Eilter can be 
derived much more easily than in the non-linear case. 
Thus, for the compactness of presentation, we will skip 
the proof of the following propositions. 


Proposition 2 The Minimum Energy filter for the 
constant acceleration model (16) and linear observation 
equations (57) is given by the equations (29) and (30) 
where the function Ak for G = (E, v) is replaced by 

Ak (G) =E^Q{Eak - yk)al , (58) 


and the components = 1,...,6 of the matrix 

Dk{G) G are given by 

{Dk{G)),,, = C{E){Er), E^ := mat,,{ef ), (59) 

with f’^{E){-) : se 3 —>■ K® given by 
matse(C''(A)(7?i)) 

T T T T\ 

:= Pr(?7i Q{Eak - yk)ak + E Qrpakak ) ■ 

Here, Q G is a symmetric and positive definite 

matrix (cf. ( |19 | ) ). All other expressions from Theorem^ 
stay unchanged. 

Since the linear observation model is a special case 
of the approach in m we only need to modify the cor¬ 
responding expressions in Algorithm which we sum¬ 
marize in the following proposition. 


The adaption of the extended Kalman Filter [12] to our 
state (16) and observation 0 equation is provided by 
the following proposition: 


Proposition 4 The extended Kalman filter from 
for our state ( [T^ and observation ( [T7| equation is given 
by Algorithm M where the expressions J{t) and Hi are 
provided in the equations (114) and ( |113 ), respectively, 
see Appendix [^ 


5 Numerical Geometric Integration 


The numerical integration of the optimal state differen¬ 


tial equation (29) requires respecting the geometry of 


the Lie group. We use the implicit Lie midpoint rule 
for integration of the differential equation of the op¬ 
timal state G* (29) as proposed in [^. We need to 


modify the method since we defined state space G as 
left invariant Lie group. Instead, in |20j . only right- 
invariant Lie groups are investigated. The adaption to 
left-invariant Lie groups is straightforward and leads 
to the following integration schemes: for a discretiza¬ 
tion tg < G < • • • < with equidistant step size 
6 = tk — tk-i for all k, we integrate the differential 


equation of the optimal state (29) using the scheme 


G(tfe+i) = G{tk) Exp(S'), (62) 

with S = (5(/(G(tfe)Exp(S/2))) 

- iB.atg{P{tk) veCg{rt{G{tk) Exp(S/2)))) . 

(63) 

For each k the matrix S is received by a fixed point 


iteration of (63). For the integration of equation (30), 
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we need to consider that this is a special kind of the 
matrix Riccati differential equation for which methods 
exist that ensure that the solution is positive definite. 
As shown in m, a numerical integration method will 
preserve positive definiteness if and only if the order 
of the method is one. By taking down (30) as general 
Riccati differential equation 


P{t) = A{t)P{t) + P{t)A{t)^ - P{t)B{t)P{t) + C{t), 

(64) 

with symmetric matrices B[t) and C{t), the implicit 
Euler integration method is given by 

P{tk+l) =P{tk) + S{^AP[tk-\-l) + P{tk+l)AP 

X (o5) 

-P{tk+i)BP{tk+i) + C), 

which can be expressed by the algebraie Riccati equa¬ 
tion for which an unique solution exists m that can 
be found by standard solvers, e.g. CARE. 


6 Experiments 

In this experimental section, we will evaluate the ac¬ 
curacy of the proposed minimum energy filter for ego- 
motion estimation. First we will provide experiments 
on synthetic data to exclude external influences and to 
show robustness against measurement noise. Then we 
will consider real world experiments on the challeng¬ 
ing KITTI benchmark and compare our method with 
a state-of-the-art method m- Finally, to evaluate the 
theoretical performance of the filter, we will also com¬ 
pare to the state-of-the-art extended Kalman filter [12] 
in a controlled environment. 


6.1 Synthetic Data 


Before considering real-life sequences, we first evaluate 
synthetic scenes to have full control on the regularity 
on the camera track. We generate 3D scenes by ray¬ 
tracing simple geometric objects (cf. Fig.[^, which also 
enables us to acquire correctly induced optical flow and 
depth maps. In order to gain a realistic camera behav¬ 
ior, we use the tracks from the KITTI visual odometry 
training benchmark which were determined by an iner¬ 
tial navigation system in a real moving car. We start 
with considering the case of perfect measurements (Sec¬ 
tion 6.1.1) and demonstrating robustness against differ¬ 


ent kinds of noise in Section 16.1.21 



Fig. 3: Synthetic sequence (top) generated by a sim¬ 
ple ray tracer. To provide realistic camera tracks we 
used ground truth trajectories from the KITTI odom¬ 
etry benchmark and computed the corresponding in¬ 
duced optical flow (mid) and the depth map (bottom). 
The corresponding color encodings for direction of op¬ 
tical flow and depth map are on the right hand side. 


6.1.1 Evaluation on Noiseless Measurements 

First, we evaluate the proposed hlter on the true opti¬ 
cal flow. To avoid overfitting, we set a relatively small 
weight onto the weighting matrix for the data term, 
i.e. Q = 0.1/n, where n is the number of observations. 
We set the weighting matrix S to the block diagonal 
matrix containing the matrices Si, i.e. 

5" = blockdiag(S'i,..., ^m), (66) 

where m denotes the order of the kinematic model and 
the Si = diag(si, si, si, S 2 , S 2 , S 2 ) with si = 10“^ and 
S 2 = 10“®. The decay rate is set to a = 2 and the 
integration step size to 5 = 1/50. 

As demonstrated in Fig. the proposed filters of 
different order show a similar rotational error since the 
ground truth rotation is often constant and influenced 
by (physical) noise. That is possibly caused by the low 
temporal resolution of 10 Hz, not being able to give 
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Fig. 5: Different noise models for the observed data (op¬ 
tical flow, cf. Fig. 1^: top left: additive Gaussian noise 
(/t = 0 , cr^ = 0 . 001 ), top right: additive uniform noise 
(/t = 0, (T^ = 0.001), bottom left: multiplicative Gaus¬ 
sian noise = 1 , cr^ = 1 ), bottom right: multiplicative 
uniform noise (/r = 1 , = 1 ). 


sufficient information on the kinematics. On the con¬ 
trary, in the translational part we can see that the 
higher-order models work significantly better than our 
first-order model [ 3 , but that third- and fourth-order 
methods perform fairly the same. From this we can con¬ 
clude that kinematics of fifth- or even higher-order will 
not improve performance regarding this kind of camera 
tracks. 


6.1.2 Evaluation on Noisy Measurements 


To evaluate the robustness against noise, we altered 
the true optical flow measurements by multiplicative 
and additive noise, each being distributed uniformly or 
Gaussian, see Fig. The proposed method determines 
camera motion using the same parameters as in Section 


6.1.1 Comparison to the ground truth is achieved using 
the geodesic distance on SE 3 in order to avoid two sep¬ 
arate error measures for translation and rotation, i.e. 


:= ||vec„(Log(F;fi£; 2 ))|| 2 . (67) 

The results in Tab. [l] show that higher-order mod¬ 
els outperform the first-order model with the excep¬ 
tion of very high noise levels where the data does not 
contain sufficient information to correctly estimate a 
higher-order kinematic. 

Remark 8 Please note that our model currently does 
not model noise on depth maps explicitly since it only 
allows additive noise on the flow measurements as in¬ 
troduced in ([TtI). However, we think that the noise term 
e should also compensate small deviations of the depth. 


Table 1: Quantitative evaluation of proposed methods 
(order 1 to 4) measuring the geodesic error (cf. (izi)) 
w.r.t. ground truth camera motion. As input data we 
used noisy flow observations with the following noise 
models: additive Gaussian (AG, /r = 0), additive uni¬ 
form (AU, /r = 0), multiplicative Gaussian (MG, /i = 1) 
and multiplicative uniform (MU, /r = 1) for different 
variances cr^. For intense noise (multiplicative: > 

10 “^, additive: > 10 “"^), the first-order method per¬ 

forms better than higher-order models since it is more 
robust against noise. In contrast, for moderate noise 
levels, higher-order kinematics are more appropriate. 


noise 

(T^ 1st order 

2nd order 

3rd order 

4th order 

MG 

0 0.2162 
0.2856 

0.2759 

0.2821 

0.2866 

MU 

0.3840 

0.3705 

0.3705 

MG 

MU 

1 O.I 597 

0.2072 

0.1644 

0.2596 

0.1485 

0.2367 

0.1423 

0.2287 

MG 

MU 

20.1417 

0.1517 

0.1184 

0.1353 

0.1041 

0.1143 

0.1011 

0.1082 

MG 

MU 

30.1283 

0.1300 

0.0987 

0.0952 

0.0844 

0.0808 

0.0808 

0.0777 

AG 

AU 

30.2859 

0.4835 

0.4355 

0.7431 

0.4318 

0.7175 

0.4385 

0.7071 

AG 

AU 

40.1598 

0.2176 

0.1695 

0.2341 

0.1688 

0.2216 

0.1701 

0.2193 

AG 

AU 

50.1384 

0.1263 

0.1157 

0.1130 

0.1010 

0.1009 

0.0974 

0.0968 

w/o 

0 0.1264 

0.0893 

0.0783 

0.0757 


6.1.3 Evaluation of Kinematics 


In the last section we showed that the proposed method 
is robust against different kinds of measurement noise. 
Now we evaluate the proposed minimum energy filters 
with higher-order kinematic model for camera tracks 
of different complexity. For this purpose, we generate 
camera tracks for the kinematic models (first to fourth 
order) by (geometric) numerical integration of corre¬ 


sponding differential equation (54) for m G {1,2,3,4} 
where we set vq = 0. In order to obtain reasonable paths 
we use non-trivial initializations for (Aq, U]*, U 3 ). 

Then we generate synthetic sequences for the different 
kinematic tracks and use the ground truth optical flow 
and depth maps as input for the proposed filters. 

The proposed method uses the parameters Q = 


O.ln ^l 2 with n = 1000; and S was chosen as in ( 66 ), 
whereas Si = 1 , S 2 = 0.001 and a = 0 . 


In Fig. we visualize the geodetical error (67) as 
well as the camera track reconstructions. It becomes 
apparent that for a camera track with constant veloc- 
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Rotational error 



Translational error 



Fig. 4: Comparison of the rotational error in degree (top) and the translational error in meters (bottom) of the 
proposed minimum energy filters with kinematic state equations of orders one (see i) and two, three and four 
(this work). The dotted lines show the error averaged over all frames. We used a real camera track from sequence 0 
of the KITTI visual odometry benchmark and generated synthetic sequences with induced depth maps and optical 
flow. The rotational errors are similar through all frames although the higher-order methods converge faster in 
the first iterations. In frames 20-90, the motion of the camera is almost constant and the filters perform similarly. 
However, the translational error of the first order method significantly changes in frames 90-150 and 175-200 
because the constant velocity assumption is violated by curves in the trajectory. 


ity (Fig. 6b) the minimum energy filter with first-order 
kinematics [3] performs best and reaches the highest ac¬ 
curacy. For the other tracks with higher-order kinemat¬ 
ics (cf. Figures [6f] and 6h|, the proposed hlters with 
higher-order kinematic model work superiorly to [9]. 


6.2 Evaluation with Realistic Observations 


ranked method on the KITTI optical flow benchmark, 
its results still contain relevant deviations from the true 
solution and thus provide realistic observation noise to 
evaluate the performance of our proposed filter. As the 
preprocessed data of [47] is dense, it causes a high com¬ 
putational effort. Therefore, we only use a sparse sub¬ 
set of data points which are selected randomly. In Sec¬ 
tion | 6 ] 2 ] 2 | we will show that a small number of observa¬ 
tions is sufficient for good reconstructions. 


In order to demonstrate that the minimum energy fil¬ 
ter with higher-order state equations also works un¬ 
der real world conditions, we evaluate our approach 
on the challenging KITTI odometry benchmark [T 8 |. 
This benchmark does not contain ground truth data 
for optical flow, and depth maps can only be obtained 
from external laser scanners. Thus, we compute optical 
flow and depth maps in a preprocessing step using the 
freely available method by Vogel et al. [IT] which only 
requires image data. Although this method is the top 


6.2.1 Quantitative Evaluation of First and 
higher-order Models 

For our quantitative evaluations on the KITTI bench¬ 
mark in Table we initialize our first jS] and higher- 
order approaches with the corresponding identity ele¬ 
ment on the Lie group, i.e. Gq = Id, and set the cor¬ 
responding matrices Pq to the identity matrices. The 
quadratic forms of the penalty term of the model noise S 
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time (frames) 

(b) geodetical error on a first-order kinematic track 



(c) reconstruced track: second-order kinematics 



time (frames) 

(d) geodetical error on a second-order kinematic track 



(e) reconstruced track: third order kinematics 



time (frames) 

(f) geodetical error on a third order kinematic track 


50 

0 


-50 




time (frames) 


(g) reconstruced track: fourth order kinematics 


(h) geodetical error on a fourth order kinematic track 


Fig. 6: Reconstruction of the camera tracks (left column) and evaluation of the geodetical error w.r.t. ground 
truth (left column) as computed by the proposed filter with kinematics of order 1, 2, 3 and 4. We evaluated 
the performance on simulated camera tracks with kinematic models of different orders: constant velocity (b)|(a)[ 
constant acceleration (d)|(c) as well as third EH and fourth [(h) |(g)| order kinematics. In the constant velocity 
scenario m the first-order filter performs best. On the other scenarios |(d)[ |(f)| |(h)[ the higher-order methods are 
superior and lead to the best path reconstructions. 
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1-5 


are set as shown in (661 with si = 10“^ and S 2 = 10 
To increase the influence of the data term, we set the 
weighting matrix to 


Q:=il2, n = 1000. 


( 68 ) 


On the one hand side, this high-weighting leads to less 
smoothed camera trajectories, but on the other hand 
side minimizes the observation error, which is desirable 
for visual odometry applications. For comparison we 
also present in Table the performance measures of 
the odometry method m- 

We emphasize that the first-order approach and 
second-order method from Theorem perform better 
in the case of camera motion reconstruction than the 
proposed higher-order (> 2) models with generalized 
kinematics from Theorem [2] The reason for that is that 
the real camera motion is influenced by model noise, in¬ 
duced by jumps of the camera, to which the first-order 
method can adapt faster. Higher-order models smooth 
the camera trajectories, which in this case is unfortu¬ 
nate. However, they will be beneficial if the actual cam¬ 
era motion behaves according to the models, as shown 
in the experiments in Section |6.1.3| 

Please note that our method currently is not de¬ 
signed to be robust against outliers in the observation. 
In contrast, the approach of Geiger et al. [19] uses addi¬ 
tional precautions to eliminate violation of the assump¬ 
tion of a single rigid body motion, see e.g. sequence 3 
in Table 121 


6.2.2 Determination of Optimal Number of 
Observations 

Since the evaluation of the functions A^. and in The¬ 
orem as well as the accurate numerical integration in 
Section[^are expensive, we are looking for a good trade¬ 
off between the number of required measurements and 
accuracy. In Tablej^we evaluate the geodetical error for 
a different number of observations n. For n = I, our pro¬ 
posed filters do not converge since they are numerically 
instable. For n = 5,..., 20, the geodetical error is fairly 
small but reaches a minimum for n = 50. For n < 5, 
the error increases because the ego-motion cannot be 
reconstructed uniquely (cf. Five-point-algorithm |55]b 
Likewise, for n > 50, the error rises due to noisy mea¬ 
surements averaged by the filter. 

6.2.3 Influence of the Decay Rate a 

In real sequences, the motion is usually not uniform 
and changes due to acceleration and curves. As demon¬ 
strated earlier, higher-order state equations that model 
accelerations, jerks, etc. usually converge faster and 


Table 3: Determination of the optimal number of mea¬ 
surements n. We evaluated the mean geodetical error 
our filter with different kinematic models (first to fourth 
order) on a short sequence (10 frames) for different 
numbers n of observations. Since the n observations are 
selected randomly, we repeated the experiment 50 times 
and averaged finally, to find a representative value. We 
found an optimal number of measurements for n = 50. 


n 

1 st order 

2 nd order 

3rd order 

4th order 

1000 

0.1205 

0.1361 

0.1311 

0.1290 

500 

0.1070 

0.1174 

0.1116 

0.1096 

200 

0.0915 

0.0945 

0.0902 

0.0890 

100 

0.0764 

0.0764 

0.0739 

0.0733 

50 

0.0667 

0.0651 

0.0638 

0.0637 

20 

0.0715 

0.0703 

0.0687 

0.0684 

15 

0.0709 

0.0691 

0.0674 

0.0672 

12 

0.0718 

0.0720 

0.0702 

0.0699 

10 

0.0749 

0.0735 

0.0716 

0.0712 

9 

0.0751 

0.0747 

0.0726 

0.0722 

8 

0.0772 

0.0762 

0.0742 

0.0738 

7 

0.0735 

0.0733 

0.0717 

0.0714 

6 

0.0786 

0.0776 

0.0757 

0.0753 

5 

0.0789 

0.0797 

0.0778 

0.0774 

4 

0.0856 

0.0859 

0.0837 

0.0831 

3 

0.0917 

0.0951 

0.0928 

0.0921 

2 

0.1005 

0.1085 

0.1058 

0.1051 


yield a better accuracy. However, higher-order models 
are delayed since it takes some time until the informa¬ 
tion from the observation is transported to the lowest 
layer. Furthermore, if the motion changes quickly, then 
higher-order models will still propagate wrong kinemat¬ 
ics. For this reason, in m a decay a > 0 rate is intro¬ 
duced and also adopted to our model. For a = 0, all 
past information is preserved in the propagation within 
the filter. For larger values of a, old information about 
the trajectory has lower influence on the filter and is 
less respected in future. 


For the experiments we use the weighting matrix 
Q = where n is the number of measurements. 

Furthermore, we use S as in (66) with the values si = 
5 • 10“^, S 2 = 5 ■ 10“"*^. The integration step size is set 
to 5 = 1/50. 


In Fig. we visualize the influence of different val¬ 
ues of a on the minimum energy filters of order 1 to 4. 
For small decay rates a, the filters will converge faster 
over time, but will also cause errors if the kinemat¬ 
ics change. On the other hand, large decay rates adapt 
more easily to spontaneous changes of kinematics. The 
filters take longer to converge, however. 
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Table 2: Quantitative evaluation of rotational (in degrees) and translational (in meters) error on the first 200 
frames of the training set of the KITTI odometry benchmark. We compared the proposed higher-order method 
(i.e. 2nd to 4th) with our first-order method from [^. As a reference method, we also evaluated the approach by 
Geiger et al. |19] . The first and second-order methods outperform the higher-order methods since they can ht more 
easily to the non-smooth ego-motion data. 



sequence 

00 

01 

02 

03 

04 

05 

06 

07 

08 

09 

10 

Sh 

(Geiger [T9])0.0272 

0.0572 

0.0255 

0.0175 

0.0161 

0.0185 

0.0118 

0.0160 

0.1166 

0.0175 

0.0147 

Sh 

Sh 

1 st order [5] 0.0284 

0.0759 

0.0188 

0.0804 

0.0165 

0.0188 

0.0122 

0.0174 

0.1142 

0.0193 

0.0205 


2 nd order 

0.0356 

0.0786 

0.0289 

0.0938 

0.0210 

0.0288 

0.0153 

0.0284 

0.1153 

0.0293 

0.0417 


3rd order 

0.0358 

0.0784 

0.0290 

0.0924 

0.0216 

0.0286 

0.0175 

0.0268 

0.1153 

0.0258 

0.0342 

s-i 

4th order 

0.0347 

0.0782 

0.0275 

0.0918 

0.0211 

0.0277 

0.0140 

0.0257 

0.1155 

0.0240 

0.0317 


(Geiger [T5110.1773 

0.1001 

0.1552 

0.1829 

0.0970 

0.1539 

0.0829 

0.1770 

0.1589 

0.1166 

0.2001 

0 

Sh 

1st order [9] 0.1773 

0.1139 

0.1504 

0.2246 

0.0836 

0.1454 

0.0765 

0.1654 

0.1444 

0.0911 

0.1829 

a; 

2 nd order 

0.1996 

0.1183 

0.1430 

0.2448 

0.0805 

0.1566 

0.0703 

0.2113 

0.1676 

0.1167 

0.2388 

0 

3rd order 

0.2402 

0.1348 

0.1872 

0.2719 

0.1090 

0.1971 

0.0875 

0.2362 

0.2053 

0.1335 

0.2628 

Sh 

4th order 

0.2795 

0.1466 

0.2223 

0.3120 

0.1479 

0.2335 

0.1045 

0.2709 

0.2318 

0.1630 

0.2956 



Fig. 7: Evaluation of the translational error (in meters) of the minimum energy filter regarding the first, second, 
third and fourth order state equation on the first 50 frames of sequence 0 of the KITTI odometry sequence. For 
small values of a, the hlter memorizes past information and converges fast, see Fig. (7a). Although higher-order 
filters converge faster, they cause oscillation due to the time delay that is required to propagate information into 
higher-order derivatives of the kinematics. Since for large values of a past information is neglected, the filters 
converge slower and the difference between second, third and fourth order models become smaller, while the 
oscillations disappear. Please note that for this experiments the weighting matrices S and Q are kept fixed. To 
further reduce the error for large a we propose to adapt the weights. 
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6.3 Comparison with the Extended Kalman Filters 


6.3.1 Experiments with Linear Observation Equation 

For the experiments in Fig. we use four observation 
equations (n = 4), and the vectors Ok in (57) are chosen 
as 


ak = el, k G [4], 


(69) 


to extract information from all directions. We gener¬ 
ate the ground truth from an arbitrary initialization 


by integration of (16) with multivariate Gaussian noise 


with mean O 12 and diagonal covariance matrix S = I 12 . 
As shown in [ 12 | . we integrate the ground truth with 
ten times smaller step sizes than the hltering equations 
of extended Kalman and minimum energy filter. Af¬ 
terwards we generate the observations with ( |61[ ) and 
Gaussian noise with covariance Q = 10 “®ll 4 and set 
the covariance matrices S and Q in Algorithm [l] to the 
same values. However, the matrix Q for the minimum 
energy filter in Proposition is set to Q = 100II 4 to 
give more weight to the observations for faster conver¬ 
gence. Note that for the extended Kalman Filter the 
choice Q — 100 14 leads to a worse performance, which 
is why we use the true covariance instead. 

As a reference, we apply our own implementation 
of the method by Bourmaud et al. [12] adapted to our 
model. The results are demonstrated in Fig.[^ We sup¬ 
pose that the main reason for the different performances 
is that we compare a seeond-order (minimum energy hl- 
ter) with a first-order (extended Kalman) hlter. 


6.3.2 Discussion on Extended Kalman Eilter for 
Non-linear Observations 


We were not able to obtain convergence of this fil¬ 
ter from a trivial (chosen as identity element of the 
Lie group) or ground truth initialization. Since the ex¬ 
tended Kalman did not converge for linear observa¬ 


tions (4.1) from wrong initializations, we presume that 


the non-linearities of our observation equations are in¬ 
tractable for the approach from m- 


7 Limitations 

Our proposed method requires good measurements in 
terms of optical flow and depth maps in order to re¬ 
construct the camera motion correctly. Although we 
showed on synthetic data that the proposed method 
is robust against different kinds of noise, it is not ro¬ 
bust against outliers, caused by independently moving 
objects that violate the static scene assumption, or sim¬ 
ply wrong computations of optical flow and depth maps. 


Making our approach robust as component of a super¬ 
ordinate processing stage, however, is beyond the scope 
of this paper and left for future work. 

In addition to optical flow, the proposed method 
requires depth information which is expensive to obtain 
if not available anyway, e.g. in stereo camera setups. 


8 Conclusion & Future Work 

We generalized the camera motion estimation ap¬ 
proach from a model with constant velocity assump¬ 
tion to a more realistic model with constant accelera¬ 
tion assumption as well as to a kinematic model which 
respects derivatives of any (fixed) order. To the authors’ 
knowledge, this has not been done so far in the fields of 
image processing and computer vision. For the resulting 
second-order minimum energy filter with higher-order 
kinematics, we provided all necessary derivations and 
demonstrated that our approach is superior to our pre¬ 
vious method [^ for both synthetic and real-life data. 
We also compared our approach to the state-of-the- 
art eontinuous-discrete extended Kalman filter on eon- 
nected unimodular matrix Lie groups m and showed 
that in both cases the minimum energy Liters is supe¬ 
rior since it converges from imperfect initializations to 
the correct solutions. 

In the future, we want to investigate how to recon¬ 
struct the camera motion (with constant acceleration) 
jointly with the eamera’s depth map from monocular 
optical flow observations. 


Appendix 

A Krouecker products ou sea 

The Kronecker products x ^ on 

sea are defined for matrices A, B £ and rj S SC3 through 

veCsc{ArjB) =: (T(8ijic B)vecst{ri) and MeCsz{Arf B) =: 
(AiSiJc B)veCsc{u). Since the explicit formulas for 
are quite uninformative, we do not provide them here. 


B Properties of SE 3 aud Q 

B.l Projection onto sea 

The projection Pr : R^’^^ —>■ sea is given by 


Pr(A) :=i diag((l, 1,1, 0)^)(Tdiag((l, 1,1,2)^) 
-TTdiag((l,l,l,0)T)) . 


(70) 
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Fig. 8: Comparison between minimum energy filter with second-order kinematics (MEF) (red, cross) and extended 
Kalman filter (CD-LG-EKF) [12] (green, square) with state equation (16) and observation equation (57) as derived 
in Properties and [^ respectively. We plotted the six components of the rigid motion of the ground truth 
(GT) (blue, circle), the extended Kalman filter, and the minimum energy filter, i.e. (wi,a; 2 j^ 2 :^ 3 )^ := 
(veCg(Logg(G))))i:6. Here, G is the corresponding element of the Lie group Q. Further, we set the discretization 
step size to 5 = 0.1. Although we initialized the extended Kalman hlter with the ground truth solution and added 
only little observation noise, it diverges after a few steps whereas the minimum energy filter converges from a 
wrong initialization to the correct solution within a few steps. The reason for that is that the approach [T2| only 
uses hrst-order approximation, whereas the minimum energy filter also includes second-order derivatives of the 
observation function. 


B.2 Adjoints, exponential and logarithmic map 

The adjoint operator ad^c (mats,c (u)) can be computed for a 
vector u g R® as follows 


vecsc (adsc (matsc(r’))f?) = adsc'^fmatsc (t>)) vecs c (r?) 
^mats„(ui;3) 03x3 


matso(u4:6) matsci(ui:3) 


dh) : 


(71) 


where matso(ui: 3 ) := (matsc(u))i; 3 ,i: 3 . This directly fol¬ 
lows from the definition of the adjoint as Lie bracket, i.e. 
adsc(5)h •= [€ih] where the Lie bracket [■, ■] : 5^3 x SC 3 —> SC 3 
is simply the matrix commutator on sea. 


vecsc (ads C 3 (mats c(u))r)) = vecsc ([matsc (j;),»?]) (72) 

= vecsc (matsc (u)r)l4 — l4r;matsc(u)) (73) 

= (matsc(j^) ®sc I 4 - I 4 matsc(u)) vecsc(»?) • (74) 


A componentwise evaluation of ( |74[ ) leads to Since R® 
is trivial, the adjoint representation on g parametrized by a 
vector V G R^^ is 


= (“*"(;"> “;:;). a-.) 

The exponential map Exp 3 g;^ : sea —> SE 3 and the loga¬ 
rithmic map on SE 3 can be computed by the matrix exponen¬ 
tial and matrix logarithm or more efficiently by the Rodrigues ’ 
formula as in 1341 p. 413f ]. 

Then the exponential map Expg : sea —> SE 3 for a 
tangent vector rj = {r]\,ri 2 ) S g and the logarithmic map 
LogQ : SE 3 —>■ sea for G = {E,v) G Q are simply 

Expg(T;) =(ExpgB^(j?i),»;2) G d, 

Logg(G) =(LoggB^(F;),t;) G g, 


(76) 

(77) 
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and similar for higher-order state spaces. 


B.3 Vectorization of connection function 

Following [T] Section 5.2], we can vectorize the connection 
function lj of the Levi-Civita connection V for constant rj, 
^ S 0 in the following way: 

vec£|(w^^) =vec^{u{r),i)) = veCg{Vr,0 = -^vec„(e) veco]??), 

(78) 

where Fx is the matrix whose {i,j) element is the real-valued 
function 

(79) 

k 

and rtf, are the Christoffel symbols of the connection function 
bj for a vector 7 E Similarly, permuting indices, we can 
define the adjoint matrix F* whose (i,j)-th element is given 
by 

(80) 
k 

This leads to the following equality: 

vecg (w^^) = vec£, (§) . (81) 

If the expression ^ in | |78| ) is non-constant, we obtain the fol¬ 
lowing vectorization from ^ Eq. (5.7)], for the case of the 
Lie algebra sea, be. 

vecst{\7nx^{x)) 

=A.ec„(e(i)) vececirix) -I- dvecse(^(x))[vecsc(r73,)] 

-^veCg (C(:c)) VCCg c (ha:) + I](7 a:)i veCj;e(d 5 (a:))[E']) 
i 

= A.eC 5 (e(a:)) veCsc(ha:) + D c{r)x) , (82) 

where the entries of the matrix D g R®^® can be computed 
as 

(b5)io = (veCsc(d 5 ( 3 ;)[£^])),, = mat^c(e®) , (83) 

where e| denotes the j-tli unit vector in R®. 

C Proofs 

Proof (of Proposition The tangent map is simply the dif¬ 
ferential or directional derivative. For Gi = {E\,v\),G 2 = 
{E 2 ,V 2 ) G 6 it holds Tq^Lq^ : Tq^G '^Lq^(G^)G- Thus, we 
can compute it for a p = (E 2 hi)h 2 ) S = Te,^ SE 3 xR® 

as follows 

Tg^Lq^ ori = dLci (G 2 )[h] 

= lim T-i(LG,(G 2 -hr» 7 )-LG,(G 2 )) 

= lim t“^(L(£;j^„j)((E 2 -|-TE 2 hi,r 2 -|-r?j 2 )) 

r—>^0 + 

— {EiE 2 ,vi -f 112 )) 

= lim ((E 1 E 2 -h TEiE 2 hii+ ^^2 + TT^a) 

r—^0 + 

— {EiE2,vi -H 112 )) 

=(GiG 2 hi,h 2 ) 6 Ta^a^g = Te^^(G2)G ■ 


For G2 = Id = (14,03) and rj = (hi,h2) S 0, it follows 

TiuLgi o h = (Eihi: h2) = T(Bi, 06) (hi: h2) =: G±ri G Tq^G ■ 

Note that the adjoint of the tangent map of Lq at identity can 
be expressed as inverse of G = {E,v), i.e. for rj = (171,172) S 
TcG and C = (5i,52) S 0 

{ 7 idTGh:C)id ={ri,TiaLai)G 

= {hl: ®Cl>B + (h2,?2> 

= {E“^hl:?l)ld..3 + (h2,52> 

= (-b(£;-l,06)7: S)ld ■ 

Thus, Ti^LqT] = 7 /(£;-i Ob)^' *bl use the shorthand 
G~^ri := Ti^Lq for the dual of the tangent map of Lq at 
identity. □ 

Proof (0/Lemma Q|) Since p = {pi,p2),v are independent of 
E the gradient di 7 f“(G = {E,v), p,t) can be computed sep¬ 
arately in terms of E, i.e. for 17 = (^171,172) G TqG 

n 

diH-{G,p,t)[rj] =(^dE^e-°‘^^-^»'>(j 2 \\yk-hk{E)\\l)[vi], 

k=l 

- d„ (711, matji c (ii)> [h2]) • 

The directional derivative regarding v can be computed by 
the usual gradient on R® which is given by 

-d„(711, matsc(r)>[h2] = - (veCsc (/n), d„i;[r72]) 

(o4j 

= (-veCsc(Mi)>h2> , 

such that d„ (711, mats c (d)) = — vecsc(7ii). For the directional 
derivative of 'H~ we first consider the directional derivative 
of hf^{E). Since hi^{E) can also be written as 

hk{E) := ((et)TE-isfc(f))-i/E-i( 7 fe(t) , / := (J ? g g) . 

(85) 

the directional derivative (into direction ^) can be derived by 
the following matrix calculus. 


d/ife(E)[C] (86) 

=d(((et)TE-ig^)-i)[5]/E-isfc 

+ {{eiV E-^gk)-^d{iE-^gi,)[^] 

= - «:-M((et)TE-igfe)[€]«:-l7E-igfc 
-hic-i/d(E-i)[5]<7fc 
= - Kfe "(et)^d(E-i)[5]g;,«:-i7E-igfc 

+ K-^id{E-^mgk 

= - Kfe ^(e|)^(-l)i?-M(E)[5]E-igfc«:-i/E-igfc 
-h«:-i/(-l)E-id(E)[5]E-igfc 

=K-^{ei)^E-^^E-^gjE-^gk - IE-^^E-^gt, , (87) 
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where = Kfc(-E) := (eg)^_E Then for the choice 

^ = Erii we find that 


n 


( 88 ) 


= - X^tr((Kj,^((e|)^?7rE ^gk)IE ^g^- K^^IgiE 

fc=l 

■ iVk - hk{E))^Q) 

n 

= ^ - K~^{{e‘^ygiE-^gt,)IE-^gk) 

k=l 

■{yk-hk{E))^Q) 

n 

= tr((K“^/»?r-E“^gfe - K.~^IE-^g^.{ei)^giE-'^gk) 

k=l 

■{yk-hk{E)yQ) 

n 

= Y1 fCfc ^-fE-^5fc(e3)^)hiE-igfc(gfe - hfc(E))^Q) 

fc=l 

n 

= ^ tr(E-igfe(gfe - - K-2/i?-igfe(et)T)r,i) 

fc=l 

n 

= I] ( {i^k^I - H^IE-^9k{ei)^fQiyk - hk{E))g^E-^,gi) w 


we gain the following equalities 

vec0 {Z{G*,t) o -h Z{G*,t) o i)»?) 

— 

®R'(t)veC(,(V_d^„-(C,. P t))7 

- ^Z(G* ,t)-^ort(G*)V -h V,,d 2 'R“(G*, 0 , t)) 

'^R'(t) vecj, (-[d2'R“ (G*, 0, t), g]) 

- Vj.-a(t-to)2(G.g)-lort(G*)h 
®R'(t)(vec£,([/(G*),»?]) 


■^vec,(e-“<*-*o)Z(G*,t)-'ort(G*)) 


(h)) 


= -.A^(E) 


^K{t){veCg{[f{G*),g]) 

+ vec„(rt(G*))) (’l) 

®i^(t)(ad--(/(G*)) 

veCg (rtCG*))') ^^^0 (^) 

= :R'(t)BveCg(g) . (93) 

The claim follows from the fact that the adjoints and the 
Christoffel symbols on R® are zero. 

Since this expression is dual to the expression in 1. the 
claim follows by using its transpose. 

Recall that the Hamiltonian in \27\ is given by 


(89) 


Here we used that the trace is cyclic. We obtain the Rieman- 
nian gradient on SE3 by projecting (cf. ^ Section 3.6.1]) the 
left hand side of the Riemannian metric in | |89[ ) onto SE3, 
which is for G = {E, v) 


dEn-(G,M,t) =e-“(*-*«)Pr£;(EAfe(E)) 

=e-‘^(^-^o)J2EPr(Ak(E)), 


(90) 


n 

H-{{E,V),g,t) = - hfe(if)||^) 

k = l 

- ^(/ri, mats c (S'“^ vecs c (Mr ))>id 

+ (M 2 ,S^V 2 >) - (Mr,mats(,(F)>id ■ 

The Riemannian Hessian w.r.t. the first component can 
be computed for G = {E,v) &Q, g= (5?r,»72) £ 0 and the 
choice /r = (/ri,/r2) = (64x4, Oa) as 


with Ai,{E) := (k- 1/ - /c^^/F;-igfe(e4)T) ' Q(y^ _ 

hk{E))g^E~^, and Pr^ : GL4 ^ Tg SE3 denotes the 
projection onto the tangential space Tg SE3 that can 
be expressed in terms of Prg(E') = EPr('). Besides, 

Pr : GL4 —>■ se3 denotes the projection onto the Lie algebra 
se3 as given in ( |70[ |. 

Putting together (|84| and (|90| results in 


di7f-(G,M,t) = 

n 

^£;Pr(^fe(E)),-veCsc(Mr)) e TaG . 


e“(‘ *"^vec0(G ^ Hessi "R (G, m, t)[G?7]) 

= vecg (g -1 VG„di'H-(G, 0, 

(94) 

= veCg(^V,,G“Mi7f(G,0,t)^ 

(95) 

n 

= vecg (v,,(^ Pr(Ai,(£;)), vec,;c 

(04X4))) 

k = l 


(96) 


(91) 


Proof (of Lemma^ Eq. ( |43[ ) can be easily found by consid¬ 
ering a basis of sea and the fact that Z is a linear operator 
on the Lie algebra. Since the resulting matrix K(t) vecg (g) := 
Z(G* ,f) o rj depends only on t, the equation ( |44| ). Eq.( |45| l is 
trivial since Z is linear. 

1. With the symmetry of the Levi-Givita connection, i.e. 

(92) 


n 

= veCsc(V,,i Pr(Afe(£;))), 06 ) 

fc=l 
n 

A: = l 


(97) 


Here, line ( |94| follows from the general definition of the 
Hessian (cf. [jj Def. 5.5.1]). Line ( |95l ) holds because of 
the linearity of the affine connection, the equation ( |96| l 
results from insertion of the expression in Lemma ^ and 
( |97| l can be achieved with ( |82| ). 

As next we calculate the differential dPr(Afe(i?))[r)i] in 
(|97ll for an arbitrary direction g±. Since the projection 


Iv,^] = 
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is a linear operation (cf. ( |70[ l), i.e. dPr(Afc(iJ))[) 7 i] = 
Pr(dylj.(E)[r;i]), we require to calculate dAj.(E)[r)i]. By 
using the product rule and the definition of Aj. from ( |32[ ) 
we obtain 

dAfe(i?)[,;i] 

=d{{K-^i - K-^iE-^9kiei)^yQ{yk - hk{E))glE-^) [7,1] 
= {A{Kl^i-K-^iE-^gk{ei)^)^[vi]Q{yk-hk{E))gjE-^) 
+ («-!/-K-2/E-ig;,(et)T)TQ((_dhfe(E)[7?i])gJi?-T) 

+ {{yk-hk{E))glAE-^[g^])'^ . 

(98) 

The directional derivative of 
is 

d(7C-V'-K-2/E-lgj,(e4)T)[^,] 

= - K“^(e|)^dE“^[77i]gfcl 

- K~^idE-^[q{\gi,(ei,y 
=K-^{ei)^E-\E-^gJ 

- 2K-^{ei)^E-^mE-^gklE-'^gkiei)^ (99) 

+ K-^iE-^giE-^gkiel)^ . 

By inserting the directional derivatives ( |99[ ), ( |87[ l and 
dE“^[r;i] = —{E~^giE~^)^ into ( |98[ ), we obtain the 
vector-valued function (^'^{E){-) : SC3 R® defined as 

matjc(C'“(-E)(r;i)) := Pr(dAfc(£;)[T;i]) (100) 

= PT(^(^K-\ei)^ E-^grE-^gJ 

- 2K-^iei)^E-^g^E-^gjE-^gk{ei)^ 

+ K-^iE-^giE-^gi,{ei}^y Q{yi, - hk{E})g^E-^ 

+ {k-^I - K-^iE-^gk{ei}^)^Q(^{K-^iE-^giE-^gk 

- ^^^^{eiyE-^giE-^gjE-^gk)gJ 

- {yk-hk{E))g^E-^gjE-y'^ . 

Using the basis of SC3, with E^ := matsc(e®) 

we define, as in ( |83[ ), the following matrix D^.{E) e M®’^® 
with components 

{DkiE))ij -.= cH^n ■ (101) 

By using the equation ( |82[ | we find that 

veCe,(Vy, Pr(Ak(E))) = (fp^(A^^(E)) + Ek(E)) vecsc(gi) ■ 

Insertion of this expression into | |97[ ) leads finally to the 
desired result, i.e. 

^a(t-to) (G“^ Hessi 'H~ (G, g, f)[G? 7 ]) 

= fEfc=i(l^vec„(Pr(yi;,(B)))+E>*,(E)) Oexe')^ gg / ■> 

V 06X6 Oaxey 


4. The Riemannian gradient of the Hamiltonian regarding 
the second component is at zero, thus we obtain 

d2R-(G,0,t) = (-mat^e(f^),0) = -/(G). (102) 

Computation of differential regarding the first component 
at ?7 = (£ 771 , 772 ) S TgG results in 

di(d2R-(G,0,t))[77] = -d/(G)[77] 

= - d(i5,„)(matsic(j^),0)[77] 

= - (matjic(72),0) . 


Finally, we compute the complete expression which is for 
7 = {m,m) e 0 and G* = {E,v) e g 


vecj, (Z(G*, i) o di (d 2 'H-)(G*, 0, t) o TmLg.t?) 


=K{t) vecg (di (d 2 R-)(G*, 0, t) [E 771 , 772 ]) 

= - £(*) veCg((matsE(772),0)) 


= - K{t) 


/Oaxe l6 \ 
\0ax6 06x6 J 


vecg(77) . 


(103) 


5. The following duality holds 


d2(diR-(G*,0,t)) =(di(d2'H-(G*,0,t)))* 
= -(dG./(G*))‘, 


(104) 


as well as the following duality rule for linear operators 
/, g : 0 —^ 0 * (i.e. /*, g* : 0 —^ 0 * by the identification 
0 “ = 0 ) and e 0 , 


{{ 9 * ° /*)(»?).Old = {f*{v),g{0)id 
={v, {f°9){i))id = {{f ° 9)*{9),0id , 


(105) 


from which follows 


( 9 * o /*) = if°9)* ■ 


(106) 


Note that for 0 = sea we replace the Riemannian met¬ 
ric (■,■} by the trace, and that the dual notation can be 
replaced by the transpose. 

Applying the vecg — operation for g S 0 gives 


!(^Id 

LJ., od2(diR- 

]104|l 

- veCg(TidTG. 


- veCg((d/(G*; 


_ /06x6 Oaxe' 


V ^6 Oaxey 


/06x6 06x6^ 


V le 06x6; 


veCg {Z{G* ,t) o g) 
A(t) vecg(g) . 


6 . It holds for g = (gi,g 2 ) S 0 and the definition of the 
Riemannian Hessian that 


Hess 2 'H {G, g,t)l'n] ='^{G,g,t). (107) 

The Riemannian gradient of the Hamiltonian regarding 
the second component can be computed for G = (E, v) g 
g as 


d2n-{G,g,t) 

= (—matjc (S)”^ vecsdgi)) — mat^c (ti), (108) 



Second-Order Recursive Filtering on the Rigid-Motion Lie Group SE 3 Based on Nonlinear Observations 


23 


Inserting ( |108[ ) into ( |107[ l results in 
Hess 2 77" (G, t) [ri] 

= ~ -|-matjic(u),S^V 2 ) 

= - Prg (^d^(niatec(>Sf ^ vecsc(/rr)) + matsc(r^))[??l, 

d^,(S^V 2 )W) 

= - ^Pr(matsE(S“^ vecjic (??i))), 

= - ^matec(5“^ veCsc(T;i)),S“\2) , 

where Prg : x R® —>■ g denotes the projection onto 

the Lie algebra g. Note that the second component of the 
projection is trivial. 

This result coincides with m where the Hessian of the 
Hamiltonian regarding the second component is com¬ 
puted directly. Applying the vecg —operation leads to 


veCg(Hess2'R {G, ti,t)[TiaLcv]) 

= - 6 “^“*“) vecg (^matjic(<S’j"^ vec^c (r;i)), 5^^772^ 

= _e«P-to)((5-i^ec,,(r7i))T, (52-^2)^)^ 

-'““■‘"’(or/sF);"’*”'' 

= :S-l 

Now we apply the vecg-operation to the expression 
Z(G*, t) o Hess 2 H- (G* ,Q,t) o Z(G* ,t)-. 


E.l Derivations for non-linear Observations 


The expression of Hi that is defined in [H Eq. (59)] is simply 
the Riemannian gradient of the observation function hj., i.e. 

n 

Hi := ^dhfc(G(tO). 
fc=r 

where hj. is defined as in ( |85[ ); and the dhj. can be computed 
component-wise (for j = 1, 2) for G(i;) = {E{ti),v{ti)) by the 
directional derivative for a direction Grj S TcQ- 

dh{{G)[Gr,]=d{{eiE-^gi,)-^ep-^gk)[{Er,ur, 2 )\ (109) 

=K~'^eirn_E-^gke‘^E-^g^, - e^g^E-^g^. ( 110 ) 

={{K-'^E-^g,,elE-^gkei - E-^g^ej)^ (111) 

=:{pi(G),7?i>, (112) 

where the second last line follows from the definition of the 
Riemannian metric on SE 3 , i.e. {g,i)id = iT ^and the fact 
that the trace is cyclic. By projection of p].{G{ti)) onto the Lie 
algebra SC 3 and by vectorization, we obtain the Riemannian 
gradient. Stacking the vectors leads to the Jacobian Hi g 
r 2 x 12 ^ which is provided through 


fj /^veCsc(Pr(pJ(tO))^ 0rx6\ 

‘ ~ ^^W,,{Pv(pl{ti)))^ OixeJ • 


(113) 


Next, we consider the calculation of the function J{t) in 
Alg. a in line[^ Following 1121 . J(t) can be calculated as 


veCg (^Z{G*,t)o Hess 2 "R" (G*, 0, f) [Z{G *, t) (p)]) 
=A(t)vecB(Hess 2 Jf(G*,0,t)[Z(G*,t)(r7)]) 
= - vec0(Z(G*,i)(7?)) 

= - veCo(7?) . 


D Christoffel symbols 


j(t) = Fit) - adg (/(G(t))) + ^G{S), (114) 

where the differential of F(t) = d/(G(t)) can be computed as 


□ 


F(t) = 


/Oexe le \ 

yOexe Oexe J 


(115) 


For a diagonal weighting matrix S, we find that in | |114| ) the 
function G can be computed for diagonal weighting matrices 
S as 


The Christoffel symbols k G {1,... , 6 } for the Rieman¬ 

nian connection on SE 3 are given by 



and zero otherwise. Note that this Christoffel symbols are 
similar to these of the kinematic connection in |52| . However, 
for the Riemannian connection, we need to switch the indexes 
i and j. 

E Derivations for Extended Kalman Filter 

The function <P : R^^ ^ jjj Alg. is 

<P(v) = f'^SE 3 («l: 6 ) 06 x 6 '\ 

^ V “ 6 X 6 l 6 ) ’ 

whereas the function •PsE^ is given in 1421 Section 10] (cf. 1121 
Eq. (17)]). 


C(S)= /^(o3X3 2 ') “®X6'\ ^ (115) 

V 06 x6 06x6/ 

where .= = - diag( (S 22 J- S 33 , 5ii + S 33 , Srr + 822 )^), and 
the adjoint in (|114|| can be computed with (|75[). 
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